classdef MultVecAttDetTest < matlab.unittest.TestCase
    %MULTVECATTDETTEST 多矢量定姿及其误差测试
    
    properties
        NAtt
        NSampPerVecGrp
        NVec
        CFM
        qMF
        MaxVecMag
        MaxVecErrMag
    end
    
    methods(TestClassSetup)
        function createInput(testCase)
            testCase.NVec = round(logspace(log10(3), log10(100), 15));
            testCase.NSampPerVecGrp = 100;
            rotAngle = [0 0 0; pi/4 0 0; pi/2 0 0; pi 0 0; 0 pi/2 pi; pi/4 -pi/4 pi/4; pi/2 -pi/2 pi/2; pi -pi pi];
            testCase.qMF = quatwrap(angle2quat(rotAngle(:, 1), rotAngle(:, 2), rotAngle(:, 3)));
            testCase.CFM = quat2dcm_cg(testCase.qMF);
            testCase.NAtt = size(testCase.qMF, 1);
            testCase.MaxVecMag = 10;
            testCase.MaxVecErrMag = 0.5;
        end
    end
    
    methods(Test)
        function vldMultVecAttDet(testCase)
            nVecGrp = length(testCase.NVec);
            betaFMMTrue_davq = NaN(3, testCase.NSampPerVecGrp, nVecGrp, testCase.NAtt);
            betaFMMTrue_SVD = NaN(size(betaFMMTrue_davq));
            betaFMMTrue_QUEST = NaN(size(betaFMMTrue_davq));
            betaFMM_davq = NaN(size(betaFMMTrue_davq));
            betaFMM_SVD = NaN(size(betaFMMTrue_davq));
            betaFMM_QUEST = NaN(size(betaFMMTrue_davq));
            betaMFF_davq = NaN(size(betaFMMTrue_davq));
            betaMFF_anal = NaN(size(betaFMMTrue_davq));
            dqMFOpt_davq = NaN(4, testCase.NSampPerVecGrp, nVecGrp, testCase.NAtt);
            dqMFOpt_anal = NaN(size(dqMFOpt_davq));
            dqMFOptDiff_anal2Davq = NaN(size(dqMFOpt_davq));
            dlambdaMax_davq = NaN(testCase.NSampPerVecGrp, nVecGrp, testCase.NAtt);
            dlambdaMax_anal = NaN(size(dlambdaMax_davq));
            for iAtt = 1:testCase.NAtt
                for iVecGrp = 1:nVecGrp
                    for iSamp = 1:testCase.NSampPerVecGrp
                        % 生成输入矢量
                        vF = rand(3, testCase.NVec(iVecGrp))*2*testCase.MaxVecMag - testCase.MaxVecMag;
                        vM = testCase.CFM(:, :, iAtt) * vF;
                        dvF = rand(size(vF))*2*testCase.MaxVecErrMag - testCase.MaxVecErrMag;
                        dvM = rand(size(vM))*2*testCase.MaxVecErrMag - testCase.MaxVecErrMag;
                        vTildeF = vF + dvF;
                        vTildeM = vM + dvM;
                        
                        % 真值输入，测试零误差精度
                        [betaFMMTrue_davq(:, iSamp, iVecGrp, iAtt), ~, betaFMMTrue_SVD(:, iSamp, iVecGrp, iAtt), betaFMMTrue_QUEST(:, iSamp, iVecGrp, iAtt), ~, ...
                            ~, lambdaMax_davq, K_davq] = MultVecAttDetTest.calcAttAndErr(vM, vF, testCase.CFM(:, :, iAtt), testCase.qMF(iAtt, :));
                        
                        % 带误差输入，测试实际误差与误差分析结果的符合性
                        [betaFMM_davq(:, iSamp, iVecGrp, iAtt), betaMFF_davq(:, iSamp, iVecGrp, iAtt), betaFMM_SVD(:, iSamp, iVecGrp, iAtt), betaFMM_QUEST(:, iSamp, iVecGrp, iAtt), ~, ...
                            qHatMF_davq, lambdaHatMax_davq] = MultVecAttDetTest.calcAttAndErr(vTildeM, vTildeF, testCase.CFM(:, :, iAtt), testCase.qMF(iAtt, :));
                        dqMFOpt_davq(:, iSamp, iVecGrp, iAtt) = quatdiff_add(qHatMF_davq', testCase.qMF(iAtt, :));
                        dlambdaMax_davq(iSamp, iVecGrp, iAtt) = lambdaHatMax_davq - lambdaMax_davq;
                        [betaMFF_anal(:, iSamp, iVecGrp, iAtt), dqMFOpt_anal(:, iSamp, iVecGrp, iAtt), dlambdaMax_anal(iSamp, iVecGrp, iAtt)] ...
                            = multvecattdeterr(vM, vF, dvM, dvF, testCase.qMF(iAtt, :)', lambdaMax_davq, K_davq);
                        dqMFOptDiff_anal2Davq(:, iSamp, iVecGrp, iAtt) = quatdiff_add(dqMFOpt_anal(:, iSamp, iVecGrp, iAtt), dqMFOpt_davq(:, iSamp, iVecGrp, iAtt));
                    end
                end
            end
            
            testCase.verifyLessThan(logtestdatastat(testCase, abs(betaFMMTrue_davq), 'βFFMTrue-Davq绝对值', {}, 2, [], true, true), 1e-13);
            testCase.verifyLessThan(logtestdatastat(testCase, abs(betaFMMTrue_SVD), 'βFMMTrue-SVD绝对值', {}, 2, [], true, true), 1.5e-14);
            testCase.verifyLessThan(logtestdatastat(testCase, abs(betaFMMTrue_QUEST), 'βFFMTrue-QUEST绝对值', {}, 2, [], true, true), 1e-13);
            
            logtestdatastat(testCase, abs(betaFMM_davq), 'βFMM-Davq绝对值', {}, 2, [], true, true);
            logtestdatastat(testCase, abs(betaFMM_SVD), 'βFMM-SVD绝对值', {}, 2, [], true, true);
            logtestdatastat(testCase, abs(betaFMM_QUEST), 'βFMM-QUEST绝对值', {}, 2, [], true, true);
            testCase.verifyLessThan(logtestdatastat(testCase, abs(betaFMM_SVD-betaFMM_davq), 'βFFM-SVD2Davq差值绝对值', {}, 2, [], true, true), 5e-14);
            testCase.verifyLessThan(logtestdatastat(testCase, abs(betaFMM_QUEST-betaFMM_davq), 'βFFM-QUEST2Davq差值绝对值', {}, 2, [], true, true), 1e-9);
            
            [~, ~, betaMFFRelDiff_anal2Davq] = relatdiff(betaMFF_anal, betaMFF_davq, false, 2);
            logtestdatastat(testCase, abs(betaMFF_davq), 'βMFF-Davq绝对值', {}, 2, [], true, true);
            logtestdatastat(testCase, abs(betaMFF_anal), 'βMFF-Anal绝对值', {}, 2, [], true, true);
            [~, stat] = logtestdatastat(testCase, abs(betaMFFRelDiff_anal2Davq), 'βMFF-Anal2Davq相对差绝对值', {}, 2, [], true, true);
            testCase.verifyLessThan(stat{2}, 0.06);
            
            logtestdatastat(testCase, abs(dqMFOpt_davq), 'δqMFOpt-Davq绝对值', {}, 2, [], true, true);
            logtestdatastat(testCase, abs(dqMFOpt_anal), 'δqMFOpt-Anal绝对值', {}, 2, [], true, true);
            logtestdatastat(testCase, abs(dqMFOptDiff_anal2Davq), 'δqMFOpt-Anal2Davq差值绝对值', {}, 2, [], true, true);
            
            [~, ~, dlambdaMaxRelDiff_anal2Davq] = relatdiff(dlambdaMax_anal, dlambdaMax_davq);
            logtestdatastat(testCase, abs(dlambdaMax_davq), 'δλMax-Davq绝对值', {}, 1, [], true, true);
            logtestdatastat(testCase, abs(dlambdaMax_anal), 'δλMax-Anal绝对值', {}, 1, [], true, true);
            [~, stat] = logtestdatastat(testCase, abs(dlambdaMaxRelDiff_anal2Davq), 'δλMax-Anal2Davq相对差绝对值', {}, 1, [], true, true);
            testCase.verifyLessThan(stat{2}, 0.04);
        end
    end
    
    methods (Static)
        function [betaFMM_davq, betaMFF_davq, betaFMM_SVD, betaFMM_QUEST, betaMFF_QUEST, qMF_davq, lambdaMax_davq, K] = calcAttAndErr(vM, vF, CFM, qMF)
            [CFM_davq, qMF_davq, lambdaMax_davq, K] = multvecattdet_davq(vM, vF);
            [CFM_SVD] = multvecattdet_svd(vM, vF);
            [CFM_QUEST, qMF_QUEST] = multvecattdet_quest(vM, vF);
            betaFMM_davq = dcmdiff(CFM_davq, CFM, false)';
            betaMFF_davq = quatdiff_mult(qMF_davq', qMF, false)';
            betaFMM_SVD = dcmdiff(CFM_SVD, CFM, false)';
            betaFMM_QUEST = dcmdiff(CFM_QUEST, CFM, false)';
            betaMFF_QUEST = quatdiff_mult(qMF_QUEST', qMF, false)';
        end
    end
end